#include "Kalman_Filter.h"

Kalman_Filter_Recursive_Struct Kalman_Filter_Recursive(Kalman_Filter_Recursive_Struct Stu)
{   
    double kg =  Stu.e_EST_Last/(Stu.e_EST_Last+Stu.e_MEA_Now);
    Stu.Evaluation_Last = Stu.Evaluation_Last + kg*(Stu.Measure_Now-Stu.Evaluation_Last);
    Stu.e_EST_Last = (1-kg)*Stu.e_EST_Last;
    return Stu;
}

Kalman_Filter_Fusion_Struct Kalman_Filter_Fusion(Kalman_Filter_Fusion_Struct Stu)
{
    double kg =  Stu.Var_1/(Stu.Var_1+Stu.Var_2);    
    Stu.Var = (1-kg)*(1-kg)*Stu.Var_1+kg*kg*Stu.Var_2;    
    Stu.Fusion = Stu.Measure_1+kg*(Stu.Measure_2-Stu.Measure_1);

    return Stu;
}

Kalman_Filter_Prediction_Struct Kalman_Filter_Prediction(Kalman_Filter_Prediction_Struct Stu)
{
   double Result_Priori = Stu.Result_Last;
   double Covariance_Priori = Stu.Covariance_Last + Stu.Q;
   double kg = Covariance_Priori/(Covariance_Priori + Stu.R);
   Stu.Result_Last = Result_Priori + kg*(Stu.Measure_Now - Result_Priori);
   Stu.Covariance_Last = (1-kg)*Covariance_Priori;

   return Stu;
}
